/*
 * joint_states_publisher.cpp
 *
 *  Created on: Aug 3, 2011
 *      Author: zhaofei
 */
#include <map>
#include <math.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionIK.h>

class JointStatesPublisher{

public:
	JointStatesPublisher()
	{
//		std::string joint_names[] = {"left_arm_link_1_joint", "left_arm_link_2_joint", "left_arm_link_3_joint", "left_arm_link_4_joint",
//									 "left_arm_link_5_joint", "left_arm_link_6_joint", "left_arm_link_7_joint"};

		ros::service::waitForService("arm_kinematics/get_ik_solver_info");
		query_client = nh_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("arm_kinematics/get_ik_solver_info");
		ik_client = nh_.serviceClient<kinematics_msgs::GetPositionIK>("arm_kinematics/get_ik");
		joint_state_publisher_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);

		if (query_client.call(request, response))
		{
			for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
			{
				ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
				joint_iter_.insert(make_pair(response.kinematic_solver_info.joint_names[i], i));
				joint_value_.insert(make_pair(response.kinematic_solver_info.joint_names[i], 0.0));
			}

		}
		else
		{
			ROS_ERROR("Could not call query service");
			ros::shutdown();
			exit(1);
		}

		gpik_req.timeout = ros::Duration(5.0);
		gpik_req.ik_request.ik_link_name = "left_arm_link_6";
		gpik_req.ik_request.pose_stamped.header.frame_id = "arm_reference_link";

	}

	bool computeIK(double py, double pz)
	{
		ros::service::waitForService("arm_kinematics/get_ik");

		double px = 0, ox = 0, oy = 0, oz = 0;
/*		tf::Quaternion q = tf::createQuaternionFromRPY(ox,oy,oz);


		gpik_req.ik_request.pose_stamped.pose.position.x = px;
		gpik_req.ik_request.pose_stamped.pose.position.y = py;
		gpik_req.ik_request.pose_stamped.pose.position.z = pz;

		gpik_req.ik_request.pose_stamped.pose.orientation.x = q.getX();
		gpik_req.ik_request.pose_stamped.pose.orientation.y = q.getY();
		gpik_req.ik_request.pose_stamped.pose.orientation.z = q.getZ();
		gpik_req.ik_request.pose_stamped.pose.orientation.w = q.getW();

*/

		gpik_req.ik_request.pose_stamped.pose.position.x = px;
		gpik_req.ik_request.pose_stamped.pose.position.y = py;
		gpik_req.ik_request.pose_stamped.pose.position.z = pz;

		gpik_req.ik_request.pose_stamped.pose.orientation.x = ox;
		gpik_req.ik_request.pose_stamped.pose.orientation.y = oy;
		gpik_req.ik_request.pose_stamped.pose.orientation.z = oz;
		gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;

		gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
		gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;

		for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
		{
			gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position
																		 + response.kinematic_solver_info.limits[i].max_position) / 2.0;
		}

		if (ik_client.call(gpik_req, gpik_res))
		{
			if (gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
			{
				for (unsigned int i = 0; i < gpik_res.solution.joint_state.name.size(); i++)
				{
					ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
					joint_iter_[gpik_res.solution.joint_state.name[i]] = i;
					joint_value_[gpik_res.solution.joint_state.name[i]] = gpik_res.solution.joint_state.position[i];

				}
				joint_state_ = gpik_res.solution.joint_state;
			}
			else
			{
				ROS_ERROR("Inverse kinematics failed");
				return false;
			}
		}
		else
		{
			ROS_ERROR("Inverse kinematics service call failed");
			return false;
		}

		return true;
	}

	bool publishJoinStates()
	{
		sensor_msgs::JointState joint_state;
		joint_state.header.stamp = ros::Time::now();
		joint_state.name.resize(7);
		joint_state.position.resize(7);
		joint_state.velocity.resize(7);

		for(std::map<std::string, unsigned int>::iterator iter = joint_iter_.begin(); iter != joint_iter_.end(); iter++)
		{
			unsigned int i = iter->second;
			joint_state.name[i] = iter->first;
			joint_state.position[i] = joint_value_[iter->first];
		}

		joint_state_publisher_.publish(joint_state_);
		return true;
	}

private:

	ros::NodeHandle nh_;

	ros::Publisher joint_state_publisher_;
	ros::ServiceClient query_client;
	ros::ServiceClient ik_client;

	// define the service messages
	kinematics_msgs::GetKinematicSolverInfo::Request request;
	kinematics_msgs::GetKinematicSolverInfo::Response response;

	// define the service messages
	kinematics_msgs::GetPositionIK::Request gpik_req;
	kinematics_msgs::GetPositionIK::Response gpik_res;

	std::map<std::string, unsigned int> joint_iter_;
	std::map<std::string, double> joint_value_;
	sensor_msgs::JointState joint_state_;

};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "joint_states_publisher");

	JointStatesPublisher joint_states_pub;
	double length = 0.6;
	double y, z;
	int i = 100;

	ros::Rate pub_rate(5.0);
	while(ros::ok())
	{

		y = length * cos(i * (M_PI / 200));
		z = length * sin(i * (M_PI / 200));
		if(!joint_states_pub.computeIK(y, z))
		{
			ROS_ERROR("computeIK failed");
		}

		joint_states_pub.publishJoinStates();
		pub_rate.sleep();
		i--;
	}

	ros::shutdown();
	return 0;
}
